%Initialises the set of particles to be evenly spread throughout the map.
%Returns a list of particles. Each particle is a 2 element vector, the
%state (x, y, th), and the weight.
function particles=initialise(map)
    sqrt_particles = 100; %This must be the square root of max particles, used for fast computing.
    max_particles = sqrt_particles.^2; %If you change this, change sqrt_particles as well. Also change in  resample
    min_x = min(map(:, 1));
    max_x = max(map(:, 1));
    min_y = min(map(:, 2));
    max_y = max(map(:, 2));
    spacing_x = (max_x - min_x)/(sqrt_particles-1);
    spacing_y = (max_y - min_y)/(sqrt_particles-1);
    coords = ones(max_particles, 2);
    for i = 0:sqrt_particles-1
        for j = 1:sqrt_particles
            coords(j+i*sqrt_particles, :) = [min_x+i*spacing_x, min_y+(j-1)*spacing_y];
        end
    end
    inpoly = inpolygon(coords(:, 1), coords(:, 2), map(:, 1), map(:, 2));
    th = 2*pi*rand(size(coords(inpoly, :), 1), 1);
    particles = [coords(inpoly, 1), coords(inpoly, 2), th, zeros(size(coords(inpoly, :), 1), 1)];
    return
end